#include "fifo.h"

static FIFO_Instance fifo_pool[MAX_FIFO_INSTANCES]; // Statically allocated instance array


/**
 * Create a FIFO queue instance
 * @param fifo_idx Index identifying the FIFO instance
 * @param elem_size Size of each element in bytes
 * @param capacity Maximum number of elements the FIFO can hold
 * @return FIFO_SUCCESS on success, FIFO_FAIL on failure
 */
uint8_t fifo_create(fifo_index fifo_idx, uint16_t elem_size, uint32_t capacity) {
    if (fifo_idx >= MAX_FIFO_INSTANCES) return FIFO_FAIL; 

    fifo_pool[fifo_idx].buffer = malloc(elem_size * capacity);
	if (fifo_pool[fifo_idx].buffer == NULL) return FIFO_FAIL;
    fifo_pool[fifo_idx].elem_size = elem_size;
    fifo_pool[fifo_idx].capacity = capacity;
    fifo_pool[fifo_idx].head = 0;
    fifo_pool[fifo_idx].tail = 0;
    fifo_pool[fifo_idx].count = 0;
	memset(fifo_pool[fifo_idx].buffer, 0, elem_size * capacity);
	return FIFO_SUCCESS;
}

/**
 * Push an element into the specified FIFO
 * 
 * @param fifo_idx Target FIFO instance index
 * @param data Pointer to the data to be pushed
 * 
 * @return FIFO_SUCCESS on success
 * @return FIFO_FAIL when queue is full
 */
uint8_t fifo_push(fifo_index fifo_idx, const void* data) {
    FIFO_Instance* fifo = &fifo_pool[fifo_idx];
    
    if (fifo->count >= fifo->capacity) return FIFO_FAIL; 
    
    uint8_t* dest = (uint8_t*)fifo->buffer + (fifo->head * fifo->elem_size);
    memcpy(dest, data, fifo->elem_size);
    
    fifo->head = (fifo->head + 1) % fifo->capacity;
    fifo->count++; 
    return FIFO_SUCCESS;
}

/**
 * Pop an element from FIFO (zero-copy method)
 * @param fifo_idx: Target FIFO instance index
 * @return: Output parameter that will point to the element
 * @note The output pointer directly references FIFO's internal buffer.
 *       Data must be used before next FIFO modification operation.
 */
void* fifo_pop_addr(fifo_index fifo_idx) {
    FIFO_Instance* fifo = &fifo_pool[fifo_idx];
    if (fifo->count == 0) return NULL;
    void* data_ptr = (uint8_t*)fifo->buffer + (fifo->tail * fifo->elem_size);

	fifo->tail = (fifo->tail + 1) % fifo->capacity;
    fifo->count--; 
    return data_ptr;
}

/**
 * Pop an element from FIFO (standard copy method)
 * @param fifo_idx: Target FIFO instance index
 * @param output: Buffer to store the popped element
 * @return: FIFO_SUCCESS on success, FIFO_FAIL when queue is empty
 */
uint8_t fifo_pop(fifo_index fifo_idx, void* output) {
    FIFO_Instance* fifo = &fifo_pool[fifo_idx];
    
    if (fifo->count == 0) return FIFO_FAIL;
    
    uint8_t* src = (uint8_t*)fifo->buffer + (fifo->tail * fifo->elem_size);
    memcpy(output, src, fifo->elem_size);
    
    fifo->tail = (fifo->tail + 1) % fifo->capacity;
    fifo->count--; 
    return FIFO_SUCCESS;
}

/**
 * @brief Check if the specified FIFO buffer is empty
 * @param fifo_idx Target FIFO instance index (predefined enum value)
 * @note Atomic read operation ensures interrupt safety
 */
uint8_t fifo_is_empty(fifo_index fifo_idx) {
    FIFO_Instance* fifo = &fifo_pool[fifo_idx];
    if (fifo->count == 0)
		return FIFO_SUCCESS;
	return FIFO_FAIL;
}

/**
 * @brief Check if the specified FIFO buffer is full
 * @param fifo_idx Target FIFO instance index (predefined enum value)
 * @warning Full state indicates no further writes possible until read occurs
 */
uint8_t fifo_is_full(fifo_index fifo_idx) {
    FIFO_Instance* fifo = &fifo_pool[fifo_idx];
    if (fifo->count == fifo->capacity)
		return FIFO_SUCCESS;
	return FIFO_FAIL;
}

/**
 * @brief Release the FIFO buffer and reset related parameters
 * 
 * This function frees the memory of the buffer corresponding to the specified FIFO index 
 * and resets all status parameters of the FIFO to their initial state.
 * 
 * @param fifo_idx Index of the FIFO to be released
 * 
 * @return returns FIFO_SUCCESS on success
 */
uint8_t fifo_free(fifo_index fifo_idx) {
	free(fifo_pool[fifo_idx].buffer);
    fifo_pool[fifo_idx].elem_size = 0;
    fifo_pool[fifo_idx].capacity = 0;
    fifo_pool[fifo_idx].head = 0;
    fifo_pool[fifo_idx].tail = 0;
    fifo_pool[fifo_idx].count = 0;
	return FIFO_SUCCESS;
}
